#include "artoolkitapi.h"

#include <QDebug>
#include "OpenCV.h"

#include <opencv/cv.h>
#include <opencv/cvaux.h>
#include <opencv/highgui.h>

#include <iostream>
#include <vector>

#include <AR/ar.h>
#include <AR/video.h>
#include <AR/config.h>
#include <AR/param.h>
#include <AR/gsub_lite.h>

ARParam         cameraParam;
int             patternIDs[2] = {-1, -1};

int             leftnum = 0, rightnum = 0, objectnum = 2;
static double   patt_width     = 55.0;	// Per-marker, but we are using only 1 marker.
static double	patt_center[2] = {0.0, 0.0}; // Per-marker, but we are using only 1 marker.
static double	patt_trans[3][4];		// Per-marker, but we are using only 1 marker.
double          gl_para[16];
double          gl_para2[16];
int             scpara[2];
int              scpara2[2];

int             xsize = 640;
int             ysize = 480;
ARMarkerInfo*   markerInfo;
int             markerNum;




/**********************
 * Constructor
 */
ARToolkitAPI::ARToolkitAPI()
{

}


/**********************
 * Converts 3x4 to 1x16 matrix
 */
void ARToolkitAPI::argConvGlpara( double para[3][4], double gl_para[16] )
{
    int     i, j;

    for( j = 0; j < 3; j++ ) {
        for( i = 0; i < 4; i++ ) {
            gl_para[i*4+j] = para[j][i];
        }
    }
    gl_para[0*4+3] = gl_para[1*4+3] = gl_para[2*4+3] = 0.0;
    gl_para[3*4+3] = 1.0;
}


/**********************
 * It all starts here
 */
void ARToolkitAPI::ARInit(int width, int height){
    int xsize = width;
    int ysize = height;

    if (arParamLoad("include/ARinfo/camera_para.dat", 1, &cameraParam) < 0) {
        qDebug() << "Camera calibration parameters file load error";
    }
    arParamChangeSize(&cameraParam, xsize, ysize, &cameraParam);
    arInitCparam(&cameraParam);

    if ((patternIDs[0] = arLoadPatt("include/ARinfo/86.patt")) < 0) {
        qDebug() << "Pattern file load error";
    }
    if ((patternIDs[1] = arLoadPatt("include/ARinfo/patt.hiro")) < 0) {
        qDebug() << "Pattern 2 file load error";
    }
    qDebug() << "finished loading";
}


/**********************
 * Marker tracking
 */
bool ARToolkitAPI::FindShit(IplImage *cvimage){

    markerInfo = NULL;
    markerNum = -1;

    ARUint8* image= (ARUint8 *)malloc(sizeof(ARUint8) * xsize * ysize * 4);

    for(int i = 0; i < ysize; i++) {
        for(int j = 0; j < xsize; j++) {
            image[4*i*xsize+4*j+0] = cvimage->imageData[cvimage->widthStep * i + j*cvimage->nChannels+0];
            image[4*i*xsize+4*j+1] = cvimage->imageData[cvimage->widthStep * i + j*cvimage->nChannels+1];
            image[4*i*xsize+4*j+2] = cvimage->imageData[cvimage->widthStep * i + j*cvimage->nChannels+2];
        }
    }

    if((arDetectMarker(image, 125, &markerInfo, &markerNum)) < 0){
        qDebug() << "you got nothing";
    }

    int i, j, k;
    /* check for known patterns */
    for( i = 0; i < 2; i++ ) {
            k = -1;
            for( j = 0; j < markerNum; j++ ) {
                if( patternIDs[i] == markerInfo[j].id ) {
                    if( k == -1 ) k = j;
                    else if( markerInfo[k].cf < markerInfo[j].cf ) k = j;
                }
            }

            if( k >= 0 ) {
                arGetTransMatCont(&(markerInfo[k]), patt_trans, patt_center, patt_width, patt_trans);

                if(i == 0){
                    scpara[0] = (int)(markerInfo[k].pos[0]);
                    scpara[1] = (int)(markerInfo[k].pos[1]);
                } else {
                    scpara2[0] = (int)(markerInfo[k].pos[0]);
                    scpara2[1] = (int)(markerInfo[k].pos[1]);
                }
                arglCameraViewRH(patt_trans,(i == 0 ? gl_para : gl_para2), 0.01);
            }
        }


    free(image);
    return true;
}


/**********************
 * does what it says it does.
 */
int *ARToolkitAPI::GetScreenCoor(int which)
{
    return (which == 0 ? scpara : scpara2);
}


/**********************
 * Returns transformation matrices, but not used here.
 */
double *ARToolkitAPI::GetTransformationMatrix(double *matrix, int which)
{
    if(which == 0){
        for(int i = 0; i < 16; i++){
            matrix[i] = gl_para[i];
        }
    } else if (which == 1){
        for(int i = 0; i < 16; i++){
            matrix[i] = gl_para2[i];
        }
    }
    return (which == 0 ? gl_para : gl_para2);
}

